#ifndef KINEMATICS_H_
#define KINEMATICS_H_

#include "motor.h"
#include "servo.h"
#include "pid.h"
#include "math.h"
#define WheelSpacing 0.162f
#define AxlesSpacing 0.144f
#define Amplitude 3.5f
#define SERVO_INIT 1500

#define CONTROL_FREQUENCY 100.0f  //编码器数据读取频率

typedef struct
{
	float wheel_spacing;
	float axle_spacing;
	
}CHASSIS_PAPAMETER_T;

typedef struct 
{
	float Vx;
	float Vy;
	float Vz;
}CHASSIS_COMMAND_T;

typedef struct
{
	MOTOR_T motor_1;
	MOTOR_T motor_2;
	MOTOR_T motor_3;
	MOTOR_T motor_4;
	
}CHASSIS_MOTOR_PART_T;

typedef struct
{
	SERVO_T servo;
}CHASSIS_SERVO_PART_T;

typedef struct
{
	CHASSIS_PAPAMETER_T parameter;
	CHASSIS_MOTOR_PART_T motor_part;
	CHASSIS_SERVO_PART_T servo_part;
	CHASSIS_COMMAND_T command;
}CHASSIS_T;

extern CHASSIS_T chassis;
void Chassis_Init(CHASSIS_T	*chassis,float axlespacing,float wheelspacing);
void Chassis_Status_Update(CHASSIS_T	*chassis);
void Chassis_Command_Update(CHASSIS_T *chassis);

void Chassis_PidControl_Task(CHASSIS_T *chassis);

#endif